#include "lincomm.hpp"
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <fcntl.h>
#include <termios.h>
#include <libserialport.h>

LinComm::LinComm(const std::string& port, unsigned int baudrate)
    : port_(port), baudrate_(baudrate) {}

LinComm::~LinComm() {
    shutdown();
}

bool LinComm::initialize() {
    port = nullptr;
    const char *port_name = port_.c_str(); 
    
    if (sp_get_port_by_name(port_name, &port) != SP_OK) {
        spdlog::error("Failed to Get LIN port name:: {}", port_name);
        return false;
    }

    if (sp_open(port, SP_MODE_READ_WRITE) != SP_OK) {
        spdlog::error("LIN open failed: {}", sp_last_error_message());
        sp_free_port(port); 
        port = nullptr;
        return false;
    }

    sp_set_baudrate(port, baudrate_);        
    sp_set_bits(port, 8);               
    sp_set_parity(port, SP_PARITY_NONE);
    sp_set_stopbits(port, 1);           
    sp_set_flowcontrol(port, SP_FLOWCONTROL_NONE); 

    running_ = true;
    readThread_ = std::thread(&LinComm::readThreadFunc, this);
    processThread_ = std::thread(&LinComm::processReceivedData, this);

    return true;
}

void LinComm::send_lin_break(sp_port *port) {
    sp_start_break(port); 
    usleep(1300);                    
    sp_end_break(port); 
    usleep(100);                    
}

void LinComm::send_sync_byte(sp_port *port) {
    uint8_t sync_byte = 0x55;
    sp_blocking_write(port, &sync_byte, 1, 1000);
}

uint8_t LinComm::calculate_pid(uint8_t frame_id) {
    uint8_t p0 = (frame_id ^ (frame_id >> 1) ^ (frame_id >> 2) ^ (frame_id >> 4)) & 0x01;
    uint8_t p1 = ~((frame_id >> 1) ^ (frame_id >> 3) ^ (frame_id >> 4) ^ (frame_id >> 5)) & 0x01;
    return frame_id | (p0 << 6) | (p1 << 7);
}

bool LinComm::send(const std::vector<uint8_t>& data) {
    if (data.empty()) return false;
    send_lin_break(port);
    send_sync_byte(port);
    uint8_t pid = calculate_pid(data[0]);
    sp_blocking_write(port, &pid, 1, 1000);
    sp_blocking_write(port, data.data() + 1, 8, 1000);
    uint8_t checksum = lin_checksum_enhanced(data.data()+1,8,data[0]);
    sp_blocking_write(port, &checksum, 1, 1000);
    return true;
}

uint8_t LinComm::lin_checksum_classic(const uint8_t *data, uint8_t len) {
    uint16_t sum = 0;
    for (uint8_t i = 0; i < len; i++) {
        sum += data[i];
        if (sum > 0xFF) sum -= 0xFF; 
    }
    return (uint8_t)(~sum);  
}

uint8_t LinComm::lin_checksum_enhanced(const uint8_t *data, uint8_t len, uint8_t pid) {
    uint16_t sum = pid;  
    for (uint8_t i = 0; i < len; i++) {
        sum += data[i];
        if (sum > 0xFF) sum -= 0xFF;  
    }
    return (uint8_t)(~sum);
}

std::vector<uint8_t> LinComm::receive() {
    std::unique_lock<std::mutex> lock(dataMutex_);
    if (receiveQueue_.empty()) {
        return {};
    }
    
    auto data = receiveQueue_.front();
    receiveQueue_.pop();
    return data;
}

void LinComm::registerCallback(DataReceivedCallback callback) {
    callback_ = std::move(callback);
}

std::string LinComm::getName() const {
    return "LIN:" + port_;
}

bool LinComm::isConnected() const {
    return running_&&port != nullptr;
}

void LinComm::shutdown() {
    running_ = false;
    
    if (readThread_.joinable()) {
        readThread_.join();
    }
    
    if (processThread_.joinable()) {
        dataCond_.notify_all();
        processThread_.join();
    }
    
    if (port != nullptr) {
        sp_close(port);
        sp_free_port(port);
    }
}

void LinComm::readThreadFunc() {
    uint8_t buffer[12];
    size_t buf_size = 12;
    while (running_) {
 
        if (sp_blocking_read(port, buffer, buf_size, 2000) > 0) {
            std::unique_lock<std::mutex> lock(dataMutex_);
            
            for (ssize_t i = 0; i < buf_size; ++i) {
                ringBuffer_[head_] = buffer[i];
                head_ = (head_ + 1) % BUFFER_SIZE;
                
                if (head_ == tail_) {
                    tail_ = (tail_ + 1) % BUFFER_SIZE;
                }
            }
            dataCond_.notify_one();
        } else if (buf_size == -1) {
            if (errno != EAGAIN && errno != EWOULDBLOCK) {
                running_ = false;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    
}

void LinComm::processReceivedData() {
    while (running_) {
        std::unique_lock<std::mutex> lock(dataMutex_);
        dataCond_.wait(lock, [this] { return !running_ || head_ != tail_; });
        
        if (!running_) break;
        
        std::vector<uint8_t> data;
        while (tail_ != head_) {
            data.push_back(ringBuffer_[tail_]);
            tail_ = (tail_ + 1) % BUFFER_SIZE;
        }
        
        if (!data.empty()) {
            receiveQueue_.push(data);
            
            if (callback_) {
                lock.unlock();
                callback_(data);
                lock.lock();
            }
        }
    }
}